#include <ros/ros.h>
#include <iostream>
#include <scout_msgs/ScoutStatus.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>

/**
 * @brief 用于订阅和显示Scout机器人状态信息的类
 */
class SensorDisplayNode {
public:
    SensorDisplayNode(ros::NodeHandle& nh, const std::string& odom_topic, const std::string& status_topic)
        : nh_(nh)
    {
        // 订阅odom话题，获取里程计数据
        odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorDisplayNode::odomCallback, this);
        ROS_INFO_STREAM("Subscribed to odom topic: " << odom_topic);

        // 订阅ScoutStatus话题，获取机器人状态数据
        status_sub_ = nh_.subscribe(status_topic, 1, &SensorDisplayNode::statusCallback, this);
        ROS_INFO_STREAM("Subscribed to status topic: " << status_topic);
    }

    ~SensorDisplayNode()
    {
        ROS_INFO("Sensor display node destroyed.");
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber odom_sub_;
    ros::Subscriber status_sub_;

    /**
     * @brief 处理里程计数据的回调函数
     * @param msg 里程计消息指针
     */
    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        try
        {
            // 提取线速度和角速度信息
            double linear_x = msg->twist.twist.linear.x;
            double linear_y = msg->twist.twist.linear.y;
            double linear_z = msg->twist.twist.linear.z;
            
            double angular_x = msg->twist.twist.angular.x;
            double angular_y = msg->twist.twist.angular.y;
            double angular_z = msg->twist.twist.angular.z;
            
            // 显示速度信息
            ROS_INFO("[Odometry] Linear velocity: x=%0.3f, y=%0.3f, z=%0.3f", linear_x, linear_y, linear_z);
            ROS_INFO("[Odometry] Angular velocity: x=%0.3f, y=%0.3f, z=%0.3f", angular_x, angular_y, angular_z);
        }
        catch (const std::exception& e)
        {
            ROS_ERROR_STREAM("Exception in odomCallback: " << e.what());
        }
    }

    /**
     * @brief 处理Scout机器人状态数据的回调函数
     * @param msg Scout状态消息指针
     */
    void statusCallback(const scout_msgs::ScoutStatus::ConstPtr& msg)
    {
        try
        {
            // 提取机器人状态信息
            double linear_velocity = msg->linear_velocity;
            double angular_velocity = msg->angular_velocity;
            
            // 显示状态信息
            ROS_INFO("[ScoutStatus] Linear velocity: %0.3f m/s", linear_velocity);
            ROS_INFO("[ScoutStatus] Angular velocity: %0.3f rad/s", angular_velocity);
        }
        catch (const std::exception& e)
        {
            ROS_ERROR_STREAM("Exception in statusCallback: " << e.what());
        }
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "sensor_listener");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");  // 私有命名空间，用于参数读取

    // 从参数服务器获取默认话题（优先读取参数文件或launch配置）
    std::string odom_topic, status_topic;
    pnh.param<std::string>("odom_topic", odom_topic, "/odom");
    pnh.param<std::string>("status_topic", status_topic, "/scout_status");

    // 命令行参数解析（覆盖参数服务器配置）
    if (argc > 3) {
        ROS_WARN("警告：仅支持前2个参数（里程计话题、状态话题），多余参数将被忽略");
    }
    if (argc == 2) {
        odom_topic = argv[1];
        ROS_WARN_STREAM("使用命令行参数作为里程计话题: " << odom_topic << "（状态话题使用默认/参数配置）");
    }
    else if (argc >= 3) {
        odom_topic = argv[1];
        status_topic = argv[2];
        ROS_INFO_STREAM("使用命令行参数配置话题:");
        ROS_INFO_STREAM("里程计话题: " << odom_topic);
        ROS_INFO_STREAM("状态话题: " << status_topic);
    }
    else {
        ROS_INFO_STREAM("使用参数配置话题:");
        ROS_INFO_STREAM("里程计话题: " << odom_topic);
        ROS_INFO_STREAM("状态话题: " << status_topic);
    }

    SensorDisplayNode sensor_node(nh, odom_topic, status_topic);

    ros::spin();
    
    return 0;
}